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摘 要 : 为 了 解决 仓储 机 器 人 在 全 动态 环境 中 的 自主 导航 问题 ， 在 分 析 自 主导 航 技术 基础 上 建立 了 机 器 人 和 动态 障 
碍 物 的 数学 模型 ， 搭 建 了 以 2 维 激光 雷达 为 主 的 环境 感知 平台 ， 提 出 了 一 种 改进 的 人 工 势 场 法 。 在 传统 的 人 工 势 场 
法 中 同时 引入 相对 速度 和 相对 加 速度 因素 得 到 改进 的 人 工 势 场 模型 ， in 环境 中 的 自主 移动 。 设 计 
了 无 障碍 物 和 多 动态 障碍 物 两 种 移动 环境 ， 经 仿真 验证 ， 应 用 改进 的 人 工 势 场 法 进行 路 径 规划 ， 能 高 效 地 避 开 动态 
障碍 物 、 跟 踪 动态 目标 ， 且 运动 路 径 光 滑 。 
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Abstract: In order to solve the problems of warehousing mobile robots autonomous navigation in full dynamic environment, 
this paper established a mathematical model of robots and dynamic obstacles, designed a platform of environmental 
awareness based on 2-dimensional lidar, and proposed an improved artificial potential field method. It introduced the 
relative speed and relative acceleration to the traditional artificial potential field method, then designed the matlab 
simulation in full dynamic environment. It is verified that the improved artificial potential field method is used to plan path, 
which can effectively avoid the dynamic obstacles, track the dynamic target, and the movement path is smooth. 
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0 ”引言 或 近似 最 优 、 无 碰撞 路 径 的 问题 品 。 
一 路 径 规 划 需 要 包 器 人 对 环境 的 感知 和 自身 定位 言 轧 ， 在 
随 着 物流 行业 的 快速 发 展 ， 对 货物 的 仓储 、 分 拣 和 运输 ”采用 一 定 的 规划 算法 来 实现 。 近 40 年 国内 外 对 路 径 规划 进行 
都 需要 大 量 的 人 力 物 力 参 与 ， 在 当前 人 口红 利 逐 渐 消 失 的 形 了 广泛 研究 ， 并 取得 了 一 定 的 成 果 。 目 前 ， 常 用 的 路 径 规划 
势 下 ， 机 器 人 的 参与 成 为 了 人 们 探讨 的 重点 ， 无 人 仓库 、 无 ”算法 有 人 工 势 场 法 、 向 量 法 、 栅 格 法 、 蚁 群 算法 、 模 糊 神经 
人 分 拣 、 无 人 派送 等 先进 技术 应 运 而 生 ， 而 仓储 机 器 人 就 是 网络、 遗传 算法 、 模 型 预测 法 、A- 算法 85 等 ， 这 些 算 法 都 各 
无 人 仓库 中 最 重要 的 角色 四。 仓储 机 器 人 在 无 人 参与 的 情况 。 有 干 秋 ， 并 适用 于 不 同 的 应 用 场所 中 。 其 中 人 工 势 场 法 是 1 
下 自主 完成 货物 的 识别 、 装 和 卸 和 运输 ， 可 24 小 时 持续 工作 ， ”Khatib 提出 的 一 种 虚拟 力 场 法 外， 其 将 机 器 人 在 环境 中 的 运 
在 很 大 程度 上 节约 劳动 力 成 本 ， 提 高 工作 效率 ， 如 亚马逊 公 ” 动 虚拟 为 一 种 在 人 工 受 力 场 的 运动 ,目标 对 机 器 人 产生 引力 ， 
司 在 全 美的 仓库 中 运用 仓储 机 器 人 ， 每 年 节约 成 本 将 近 10 ”障碍 物产 生 斥 力 ， 引 力 和 斥 力 的 合力 控制 机 器 人 的 运动 。 该 


T 


汕 


亿美 元 。 算法 因 其 数学 分 析 简 单 、 计 算 量 小 、 路 径 光 滑 等 优点 被 广泛 
仓储 机 器 人 关键 技术 成 为 了 国内 外 研究 机 构 研 究 的 重 立 用 在 实时 避 障 和 路 径 规划 领域 。 

点 ， 自 主导 航 是 仓储 机 器 人 研究 中 最 核心 技术 ， 其 包括 对 环 在 过 去 针对 人 工 势 场 法 的 研究 中 ， 大 量 的 研究 致力 于 如 

境 的 感知 、 机 器 人 自身 定位 和 路 径 规划 几 大 部 分 。 对 环境 的 可 改进 入 工 势 场 法 7 引 以 解决 该 算法 存在 的 局 部 最 小 值 点 和 


感知 主要 融合 一 系列 传感器 信息 来 实现 , 如 超声 波 、 红外 线 、 目标 不 可 达 问 题 ， 虽 取得 显著 的 成 效 ， 但 大 多 数 都 是 应 用 于 
激光 雷达 、 图 像 等 ， 机 器 人 的 定位 可 由 GPS、 惯 性 (速度 、 静态 环境 中 ， 即 目标 和 障碍 物 都 处 于 静止 不 动 状 态 。 然 而 ， 
位 移 计 等 ) 、Wi-Fi、LBS 基站 、 环境 磁场 定位 技术 来 实现 中， ”在 无 人 仓储 应 用 环境 中 ， 仓 储 机 器 人 所 处 的 环境 是 复杂 的 、 
路 径 规划 是 自主 导航 技术 中 最 重要 的 部 分 ， 是 解决 仓储 机 器 。” ”动态 变化 的 。 首 先 ， 目 标 可 能 是 运动 的 ， 如 Fetch 和 Freight 
人 在 复杂 、 动 态 环 境 中 如 何 搜寻 出 一 条 到 达 目 标 货架 的 最 优 ”仓储 机 器 人 ，Fetch 负责 将 商品 从 货架 上 拿 下 来 ，Freight 负 


收 稿 日 期 : 2018-09-15; 修 回 日 期 : 2018-11-07 ”基金 项 目 : 重庆 市 教委 科技 基金 资助 项 目 〈(KJ1601032) ; 重庆 市 科学 技术 委员 会 基础 科学 与 前 
沿 技术 项 目 〈cstc2017jcyjA1543) ; 重庆 市 教委 科学 技术 研究 项 目 〈KJ1601001) 

作者 简介 : 罗 强 (1988-) ， 男 ， 重 庆 万 州 人 ， 讲 师 ， 硕 士 ， 主 要 研究 方向 为 智能 移动 机 器 人 、 控 制 理 论 与 工程 相关 领域 研究 〈qluosxxy@163.com) ; 
王 海 宝 (1966-) ， 男 ， 辽 宁 庄 河 人 ， 教 授 ， 硕 导 ， 主 要 研究 方向 为 机 电 系 统 设 计 ， 振 动 检测 ; 崔 小 劲 (1972-) ， 男 ， 重 庆 万 州 人 ， 高 级 工程 师 ， 硕 士 
主要 研究 方向 为 自动 控制 徐 洪 泽 (1966-) ， 男 ， 内 蒙古 呼 尔 浩特 人 ， 教 授 ， 博 导 ， 主 要 研究 计算 机 控制 ， 微 机 联 锁 . 


201901.00038v1 


国 
国 


IV 


china 


录用 定稿 


责 将 商品 运送 打包 ，Fetch 机 器 人 作为 目标 就 是 动态 变化 的 ， 
Freight 则 需要 动态 跟随 目标 。 其 次 ， 障 碍 物 也 有 动态 和 静态 
的 ， 在 无 人 仓库 中 的 货架 、 物 品 、 墙 壁 、 工 作 台 等 都 属于 静 
态 障 但 物 ， 但 同时 运行 的 其 他 仓储 机 器 人 和 工作 人 员 就 变 成 
了 动态 障 但 物 。 
针对 全 动态 环境 下 的 路 径 规划 问题 ， 国 内 外 学 者 也 开展 
了 相关 研究 ， 文 献 [9] 提 出 一 种 改进 的 路 径 规 划算 法 ， 基 于 动 
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I 


机 器 人 为 变 加 速度 运动 时 ， 机 器 人 的 速度 关系 为 
Vi =W+Aea’ (4) 
本 文中 所 有 表达 式 中 用 上 标 ”表示 与 机 器 人 相关 ， 上 标 
2 表示 与 障碍 物 相关 ， 上 标 8 则 表示 与 目标 相关 。 
1.2 动态 障碍 物 数学 模型 
在 无 人 仓储 环境 中 ， 动 态 障 碍 物 可 为 行走 的 人 、 其 他 仓 
储 机 器 人 、 移 动 的 货架 等 ， 其 运动 状态 可 以 为 匀速 、 变 速 的 


态 障 得 物 设计 了 一 种 新 的 避 障 规则 ， 有 效 实 现 避 障 和 跟踪 


标 ， 但 采用 相同 距离 的 规划 步 长 ， 即 只 考虑 规划 方向 而 未 考 
虑 规划 驱动 力 的 变化 ， 且 规划 路 径 不 光滑 ， 存 在 大 的 转角 变 


化 。 文 献 [10,11] 提 出 了 一 种 改进 的 人 工 势 场 法 ， 在 引力 势 场 
函数 中 引入 相对 速度 和 相对 加 速度 ， 有 效 解 决 动态 目标 的 跟 
踪 问题 ， 在 斥 力 势 场 函数 中 引入 相对 速度 因素 ， 来 绕 开 动态 
障碍 物 ， 但 存在 动态 避 障 效率 不 高 、 规 划 路 径 长 的 问题 。 

本 文 重点 对 仓储 机 器 人 自主 导航 技术 中 的 环境 感知 和 路 
径 规 划 问 题 。 首 先 采 用 2 维 激光 雷达 构建 仓储 机 器 人 环境 感 
知 系统 ， 实 时 检测 机 器 人 周围 的 障碍 物 姿态 ， 然 后 在 传统 人 
工 势 场 法 基础 上 引入 相对 位 置 、 相 对 速度 以 及 相对 加 速度 ， 
改进 势 场 函数 ， 从 而 解决 仓储 机 器 人 在 全 动态 环境 中 自主 导 


航 问题 。 
1 ”移动 环境 模型 建立 


1.1 仓储 机 器 人 数学 模型 
本 文选 用 的 仓储 机 器 人 模型 如 图 1 所 示 ， 后 轮 为 2 轮 差 
速 驱动 轮 ， 前 轮 为 2 个 起 支撑 作用 的 万 向 轮 。 此 种 机 器 人 系 


直线 运动 或 者 曲线 运动 ， 且 可 在 几 种 运动 模型 中 随机 切换 
[1。 障 碍 物 的 姿态 可 用 XY"=[x,y",0" 了 ccR 来 表示 ， 在 实际 应 
用 中 同样 可 采用 欧 拉 近似 法 得 到 障碍 物 的 姿态 变化 方程 : 

Xo = Xe+A® f° (qo, ws, 08) (5) 


其 中 :go elline,arc) ， 代 表 了 下 一 时 刻 机 器 人 的 运动 状态 是 


直线 运动 或 是 曲线 运动 。 如 果 4%,=line 则 为 直线 运动 ,此 时 ww 
为 线 速度 ， 若 qt4=arc 则 为 曲线 运动 ， 此 时 wr 为 角速度 。 不 
同 运动 状态 下 f°(q2u,wh,88) 也 存在 一 定 的 差异 。 
直线 运动 状态 时 ， 
we | 
f° (line, we, 08)= 区 (0) 
0 】 
其 中 :7 为 直线 运动 的 斜率 ， 可 由 y=tanb 求 的 。 
曲线 运动 状态 时 ， 
rwe coOS 0 
f°(arc, wa, 0)= sin 0 (7) 
Wa 
其 中 : + 为 曲线 运动 时 圆 弧 半 径 ， 在 随机 运动 环境 中 ，7 也 


统 为 典型 的 非 完整 约束 是 非 完 整 系统 03， 机 器 人 的 姿态 可 描 


述 为 Xr =[x’,y’,0'] CR 。 


图 1 仓储 机 器 人 模型 
Fig. 1 Warehousing robot model 


其 数学 模型 为 


入 =VrcosOr 

y=v" sinO” (1) 

Gr 
其 中 : (wx,y") 为 机 器 人 的 位 置 坐标 ， 2 为 机 器 人 的 位 姿 角 
v 为 机 器 人 的 瞬时 线 速度 ，w' 为 机 器 人 转动 角速度 。 机 器 人 
系统 的 输入 为 UV'=tv ,wleU%mcR ， 输 入 范围 为 


是 可 以 动态 变化 的 。 
Wo =W+A*a? 。 
1.3 条 件 假设 

仓储 机 器 人 的 自主 导航 系统 包括 硬件 平台 和 软件 平台 ， 
功能 上 实现 机 器 人 自身 定位 ` 外 界 环境 感知 和 路 径 规划 算法 。 
本 文 重 点 研究 仓储 机 器 人 对 外 界 环 境 的 感知 和 全 动态 环境 下 
的 路 径 规划 问题 ， 为 了 简化 分 析 ， 可 作 如 下 假设 : 
机 器 人 的 位 置 p'、 速度 w 和 加 速度 w 都 实时 可 知 ， 且 仓 
储 机 器 人 外 形 可 视 为 半径 为 R 的 圆 形 。 
标的 位 置 pz 、 速 度 vw 和 加 速度 a: 都 可 实时 检测 到 ， 
目标 的 速度 满足 | 四 加 we | 。 
障碍 物 的 位 置 p*、 速度 w 和 加 速度 a? 都 可 实时 获得 , 其 
外 形 可 视 为 为 半径 为 RR 的 圆 形 。 

假设 目标 和 障碍 物 都 为 色 加 速 运 动 ， 达 到 最 大 运行 速度 
后 为 匀速 运动 。 
2 ”激光 雷达 构建 环境 感知 平台 
2.1 激光 雷达 工作 原理 

激光 雷达 是 传统 雷达 和 激光 技术 相 结 合 的 产物 ， 它 利 
激光 载波 先 向 目标 发 射 探测 信号 ， 然 后 将 其 接收 到 的 同 波 
号 与 发 射 信号 相 比 较 ， 从 而 获得 目标 的 位 置 〈 距 离 、 方 位 和 


同时 ，w 也 可 动态 变化 ， 则 


mr : 


[= DA]x[omsots]。 在 实际 应 用 中 ， 机 器 人 的 姿态 是 动 
态 变化 的 ， 即 机 器 人 下 一 时 刻 的 姿态 为 当前 姿态 加 上 单位 时 
间 内 的 变化 量 ， 可 以 用 欧 拉 近 似 法 来 描述 机 器 人 的 姿态 变化 
关系 。 


Xi = X71 + Af’ 5) (2) 
其 中 : A 为 机 器 人 规划 间隔 时 间 ， fw,e) 是 关于 机 器 人 运 
动 线 速度 vw 和 角速度 wx 的 函数 ， 其 数学 关系 为 

| 让 cos@5 

f "(Vn, 0)=| vsin Os; (3) 
Lo 


高 度 ) 、 运 动 状态 (速度 、 姿 态 ) 等 信息 ， 实 现 对 目标 的 探 
测 、 跟 踪 和 识别 。 
利用 2 维 激光 雷达 实现 仓储 机 器 人 自主 导航 的 工作 原 
理 : 将 激光 雷达 固定 在 机 器 人 上 ， 实 时 扫描 周围 环境 ， 得 到 
周围 障碍 物 的 实时 距离 和 方位 ， 则 可 构建 与 机 器 人 为 中 心 、 
以 雷达 扫描 半径 为 最 大 区 域 的 环境 地 图 ， 多 次 测量 可 辨别 出 
障 但 物 的 运动 姿态 ， 进 而 为 机 器 人 的 路 径 规划 提供 基础 。 

2.2 激光 雷达 数据 的 获取 
本 文选 用 RPLIDAR 系列 激光 雷达 ， 测 量 距离 为 0.15~8 
米 , 扫描 角度 范围 为 0”~360”, 测 距 分 状 率 最 大 为 0.5 mm， 


[ey 
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其 中 : km 分 别 为 引力 和 斥 力 势 场 增益 系数 ，|| pp 为 机 
器 人 相对 于 目标 的 距离 ，p 直 pp" 趾 为 机 器 人 与 障碍 物 的 距 


录用 定稿 罗 强 ， 等 : 
角度 分 标 率 为 0.45”， 扫 描 频 率 为 10 赫兹 ， 即 1S 扫描 10 
周 。 其 数据 采集 模型 如 图 2 所 示 ， 经 扫描 可 确定 障碍 物 相 对 
于 激光 雷达 的 距离 p; ， 由 激光 光束 号 可 确定 方位 向 ， 从 而 得 
到 以 激光 雷达 为 中 心 坐 标 系 的 极 坐 标 数据 点 (or 办) ， 其 中 


入 = 


(j—D)*0.45°, j =1~ 801 。 


图 2 激光 雷达 模型 
Fig.2 Lidar model 
当 激 光 雷 达 与 机 器 人 几何 中 心 重合 时 ， 激 光 雷 达 的 位 置 


坐标 可 以 机 器 人 位 置 坐标 来 表示 ， 若 机 器 人 的 位 置 坐 标 为 
(xw,y") 时 ， 那 障碍 物 的 位 置 坐 标 则 为 

| Py cos 

人 四 6) 


HH. 


: 表示 同一 周期 内 的 激光 束 号 ，i 表 示 采 样 时 间 序 列 。 
采样 的 激光 雷达 数据 中 不 可 避免 地 存在 噪声 点 ， 这 样 会 


降低 障碍 物 或 目标 检测 的 精度 ， 在 实际 设计 中 需要 对 原始 数 


据 进 行 滤波 处 理 。 


率 滤波 算法 ， 


文献 [13] 提 出 一 种 自 适应 


以 激光 雷达 在 第 i 时 刻 第 i 个 扫描 线 所 测量 的 距离 mw 为 中 


心 ， 


上 的 最 大 相关 性 ， 可 有 效 地 减少 噪声 ， 又 能 够 保持 数据 边界 
、 保 留 离 群 点 。 
均值 滤波 算法 ,日 
然后 对 各 像素 进行 


点 


建立 3x3 的 数据 分 析 9 个 数据 在 时 间 和 空间 


分 析 窗 口 ， 


文献 [14] 中 提出 了 基于 高 程 信息 平坦 度 的 
先 将 激光 雷达 的 强度 信息 转换 为 灰 度 图 像 ， 
均值 滤波 处 理 。 激 光 雷 达 数 据 常 见 的 滤波 


所 


算法 还 有 基于 数学 形态 学 的 滤波 算法 、 基 于 坡度 的 滤波 算法 、 


基于 TIN 的 LIDAR 点 云 过 滤 算 法 、 基 于 伪 扫 描 线 的 滤波 算 
法 等 ， 相 关 算 法 研究 较 多 且 容 易 实现 ， 此 处 不 再 鳌 述 。 
实时 获取 的 障碍 物 位置 坐 标 可 判断 障碍 物 为 静止 或 动 


动 状态 ， 且 可 计算 出 障碍 物 的 瞬时 速度 和 加 速度 等 ， 为 
的 路 径 规划 提供 数据 支持 。 在 无 人 仓储 系统 中 ， 由 于 动 


委 运 讼 迅 汪 
并 下 让 淤 衣 
区 


E+ 
EE 


nT 


3 
3 


FP 的 多 机 器 人 协同 工作 将 会 


障碍 物 多 为 其 他 仓储 机 器 人 ， 仓 储 机 器 人 自身 具有 定位 
E, 也 可 构建 无 线 网 络 系统 ， 利用 无 线 网 络 实时 分 享 自身 
立 置 ， 从 而 可 构建 全 局 路 径 规划 环境 ， 在 无 线 网 络 
在 后 续 研究 中 涉及 。 


路 径 规划 算法 研究 
传统 人 工 势 场 法 
传统 的 人 工 势 场 法 只 考虑 机 器 人 与 目标 、 障 碍 物 的 距离 


天 数 ， 


标 对 机 器 人 产生 的 引力 势 场 与 两 者 距离 成 正比 例 关 


AN; 


引 
者 吕 


则 越 小 ， 


离 目标 越 远 产生 的 引力 势 场 越 大 ， 反 之 则 越 小， 从 而 牵 
1 器 人 向 目标 逼近 ;， 障碍 物 对 机 器 人 产生 的 斥 力 势 场 与 两 
E 离 成 反比 例 关 系 ， 离 障碍 物 越 近 产生 的 斥 力 越 大 ， 反 之 
以 达到 避 障 的 效果 051。 其 数学 函数 关系 如 式 (9)(10) 


所 示 


吕 昌 -| 


1 
Ua(p)==Klp' -ps 上 (9) 


离 ，A 为 障碍 物 斥 力 势 场 对 机 器 人 产生 影响 的 距离 界限 ， 当 
P>po 时 机 器 人 受 斥 力作 用 ， 反 之 则 不 受 。 
引力 和 斥 力 分 别 有 相 应 势 场 函数 的 负 梯 度 求 得 ， 分 别 为 
Fu(p)=-g8radlUa(p)]=-k||P" -Ps| (11) 
机 器 人 指向 目标 。 


其 方向 为 


A 


Pa(p)=-gradlU (p)]= (12) 


0， D>ADo 
其 方 向 为 由 障碍 物 指向 机 器 人 。 当 由 多 个 障碍 物 时 ， 
Fo = 六 Fw ， 最 后 斥 力 和 引力 的 合力 Bo = Fn+ Fne 为 机 器 人 


提供 下 一 步 路 径 规 划 方 向 和 力 。 

传统 人 工 势 场 法 适用 于 静态 环境 的 路 径 规 划 ， 而 对 于 全 
动态 的 仓储 应 用 环境 ， 目 标 和 障碍 物 都 在 于 随机 和 运动， 运动 
形式 和 姿态 都 实时 变化 ， 将 无 法 满足 规划 要 求 。 
3.2 改进 人 工 势 场 法 
3.2.1 改进 的 引力 势 场 范 数 和 引力 
当 目 标 为 运动 状态 的 取 件 机 器 人 时 ， 不 仅 对 仓储 机 器 人 
的 位 置 作出 要 求 ， 还 需要 其 与 目标 保持 相同 的 速度 和 相同 的 
运动 趋势 ， 即 机 器 人 与 目标 的 相对 速度 和 相对 加 速度 为 零 。 
传统 的 引力 势 场 函数 是 无 法 满足 要 求 的 ， 需 要 进一步 改进 。 
为 了 解决 仓储 机 器 人 的 速度 和 加 速度 需 和 目标 保持 相同 
运动 趋势 的 问题 ， 特 将 机 器 人 与 目标 点 的 相对 速度 和 相对 加 
速度 因素 引入 到 传统 引力 势 场 函数 中 5 1， 如 式 (13) 所 示 。 


1 1 
Un(p,v,0)==ko |p" -ps +k lv vl + 
2 2 
(13) 


1 
~kalla’—as 上 
Shklar arl 


其 中 :Kk, 、 和 都 为 正 因 常数 因子 ，p'、ps、vw、vs、 
a 和 as 分 别 为 机 器 人 和 目标 的 位 置 、 速 度 和 加 速度 。 
P"- 户 为 机 器 人 与 目标 之 间 的 几何 距离 ， | -vw 为 机 器 人 
与 目标 之 间 的 相对 速度 大 小 ，lle -4 为 机 器 人 与 目标 之 间 
的 相对 加 速度 大 小 。 
式 (13) 可 见 ， 当 且 仅 当 机 器 人 与 目标 的 相对 位 置 、 相 
对 速度 和 相对 加 速度 三 项 都 为 零 时 ， 引 力 势 场 函数 才 为 零 ， 
否则 将 会 持续 为 仓储 机 器 人 提供 引力 作用 


其 引力 函数 可 由 引力 势 场 函数 的 负 梯 度 求 的 ， 其 推导 如 
式 (14) 所 示 。 
Fan(p,v,a)=—grad[Uan(p,v,a)]=—gradp[Uan(p,v,a)] 14 
— grad,[Uan(p,v,a)]— grada[lUan (p,v,a)] (14) 
令 
OUan(p,v,a) 
ar(D) = 一 81ddpn[UVw(PV dg] = 一 一 一 一 一 
Fam (Pp)=—grady [Uan(p,v,a)] Bp (15) 
= 一 如 下 六 一 如 
其 方向 为 由 机 器 人 指向 目标 点 。 
同 理 ， 令 
Fn) =—grad Un om) = (16) 


= vl 


其 方向 为 由 目标 点 速度 向 量 与 机 器 人 速度 向 量 之 差 向 量 
Vs 一 的 方向 。 


OUw (Xx,V,a) 


Fna(a)=—gradalUan (xX,Vv,a)]=— Ba (17) 


= ar -ae 
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其 方向 为 由 目标 点 加 速度 向 量 与 机 器 人 加 速度 向 量 之 差 标 可 记 为 ,yw) ， 引 入 齐 次 变换 矩阵 ， 可 得 到 机 器 人 在 新 | 
向 量 -az 的 方向 。 坐标 中 的 变换 关系 如 式 (23) 所 示 。 
则 机 器 人 受到 的 引力 为 10—x ||cosw -snw 0 
Fan (XsV, 4) = Fen (X) + Fa (Vv) + Fana(a) (18) M=I01-y|lsinwy cosy 0 (23) 
引力 计算 过 程 及 各 向 量 的 关系 如 图 3 所 示 。 05 J 0 1 
其 中 为 障碍 物 相 对 于 机 器 人 速度 方向 与 原 坐标 系 的 夹 角 。 
在 转换 后 的 坐标 系 中 ， 可 求 得 
cosC = 十 一 有 (24) 
由 式 (21)(24) 可 推导 出 
Fw =—VpUa =—Vp(m, ||v" —v° llcosa) 
mineino, 一 COSC) (25) 
XB + 2 
=—m, wl Ql(sinag,—coso) 
可 见 18oe Fw 上 Isinal , 即 为 机 器 入 与 障碍 物 的 相 
对 速度 在 两 者 连 线 垂 线 上 的 分 量 。 
图 3 计算 引力 过 程 同 理 ， 可 由 式 (22) 求 得 
Fig.3 Attraction force calculation process Fs =—VpU =—Vp(ma lar ~ar cosp) 
3.2.2 改进 的 斥 力 势 场 函 数 和 斤 力 | Ls 
在 全 动态 环境 中 ， 障 碍 物 也 处 于 运动 状态 ， 与 改进 的 引 Va + yo (26) 


力 势 场 函数 类 似 ， 在 斥 力 势 场 函 数 中 同样 引入 相对 速度 和 相 


对 加 速度 因素 494， 如 式 (19) 所 示 。 


了 (全 -二 -welleosc 
p Am 


2 
+malla’ —a° llcosp, 
Usp (p,V, 4) = XA 
<= po & Ra e(-—~,—~ 
p<=Po (一 7 7) 
A 
0 ,PpP>Polla#(- pi 
令 
1 1 1 
Uepp =~1p(———)? 
2 p PP 


Uo =m, ||v" —v° ||cosa 
Um =malla’ ~a? llcospB 


了， my 


™ 


分 量 ， 各 斥 力 势 场 关系 如 图 4 所 示 。 


图 4 改进 的 斥 力 势 场 向 量 关 系 


、 mw 和 ms 都 为 正 因 常 数 因子 ，w 和 分别 为 障碍 
物 的 速度 和 加 速度 。 lv -wl 为 机 器 人 与 障碍 物 之 间 的 相对 速 
度 大 小 ，le -1 为 机 器 人 与 障碍 物 之 间 的 相对 加 速度 大 小 。 
%、A 分 别 为 机 器 人 相对 于 障碍 物 的 相对 速度 矢量 和 相对 加 
速 矢量 与 相对 位 置 撩 量 的 夹 角 ， 可 看 出 改进 后 的 斥 力 势 场 函 
数 引 入 了 相对 速度 和 相对 加 速度 在 机 器 人 与 障碍 物 连 线 上 的 


(19) 


Ey 
2 


(20) 


(21) 
(22) 


Fig.4 Relation of improved repulsive potential field vectors 


为 了 便于 分 析 引 入 的 相对 速度 分 量 与 相对 位 置 


可 将 坐标 系 进行 一 定 平移 和 旋转 处 理 ， 
碍 物 中 心 点 ， 


的 关系 ， 
先 将 坐标 原点 移 到 障 
再 将 X 轴 旋转 到 障碍 物 相对 于 机 器 人 的 速度 方 
向 ， 建 立新 的 直角 坐标 系 。 在 此 坐标 系 中 ， 机 器 人 的 位 置 坐 


nl el 


—————sinp(sinp,—cosp) 
7 
| 已 om |= —ma la 


1 下 sinp1, 即 为 机 器 人 与 障碍 物 的 相对 加 


速度 在 两 者 连 线 垂 线 上 的 分 量 。 


由 式 (20) 可 求 得 
Fm =-YpUm =-YpDm 人 -二 站 
| D Am (27) 
=1p 全 二 
Do 
可 以 求 的 势 场 函数 关于 速度 和 加 速度 的 负 梯 度 函 数 ， 如 
式 (28)(29) 所 示 。 
Fepv =—VV(U epp + Urops + Uepa) 
=—VvUo =—Vv(m, ||v" —v° lcosa) (28) 
=—m, COSC 
Frepa =—YVa(U repp + Urepv + Urepa) 
=-VaU =—Ya(malla’ ~a® ||cos Bp) (29) 
=—macosp 
由 式 (25)~(29) 可 得 到 斥 力 函 数 为 
Freppi + Frepp2 + Frepp3 + Frepv + Frepa, 
Po=) mn 人 ku 人 了) (30) 


KXNA 
0, > 科 ( 一 一 ,一 
p> Plae(-3,7) 


5 展示 了 斥 力 函数 计算 过 程 和 各 函数 关系 。 

图 5 可 见 ， 改 进 后 的 斥 力 函 数 在 传统 斥 力 函数 的 位 置 
斥 力作 用 基础 上 , 额外 增加 了 Fw 和 Fm 两 个 斥 力 , 加 大 了 障 
碍 物 排斥 作用 ， 同 时 ， 在 垂直 于 位 置 斥 力 方向 上 ， 额 外 增加 
了 Fun 和 Fws 两 个 力 ， 为 机 器 人 提供 绕 行 障碍 物 的 能 


最 后 由 Fw 和 Fi 的 合力 为 机 器 人 提供 下 一 步 规划 方向 和 
力 。 
= G1) 
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i I + a * A, | vi |< vi (32) 4.2 无 障碍 情况 下 的 目标 跟踪 
~ bie, bwin 先 可 在 无 障碍 物 情况 下 根据 改进 的 引力 函数 对 目标 进 


行 跟踪 实验 ， 从 而 调节 引力 函数 中 的 相关 参数 ， 选 择 合适 的 
数值 ， 仿 真 参 数 如 表 1 所 示 。 
表 1 无 障碍 物 情况 下 的 仿真 参数 


Table 1 Simulation parameters without obstacles 


仿真 参数 名 称 及 符号 值 /( 单 位 ) 
仿真 环境 面积 s 30x 30/ Gm? 
标 初始 位 置 ps (5,27)/(m) 
标 初始 速度 vs (0.3,-0.3)/(m/s) 
标 初 始 加 速度 as (0.2,-0.4)/(m/s’) 
机 器 人 初始 位 置 p” (3,15)/(m) 
机 器 人 初始 速度 (0,0Y/(m/s) 
机 器 人 初始 加 速度 a” (0,0)/(m/s’) 
图 5 斥 力 函 数 计算 过 程 机 器 人 质量 m 10/(Kg) 
Fig.5 Repulsion function calculation process 规划 时 间 A 0.3/(s) 
士 最 大 移动 速度 yma 3/(m/s) 
4 在 相同 环境 下 经 过 多 次 实验 , 调整 、k 和 三 个 参数 ， 
为 了 验证 改进 的 人 工 势 场 法 在 仓储 机 器 人 路 径 规划 中 的 。 ”可 以 获得 不 同 的 跟踪 效果 ， 有 的 能 够 达到 快 的 响应 时 间 ， 但 
可 行 性 ， 通 过 MATLAB R2016a 分 别 在 无 障碍 情况 下 的 目标 。 存在 过 大 的 振荡 ， 增 大 跟踪 路 程 开销 ， 如 图 7 所 示 ; 有 的 追 
跟踪 和 有 障碍 物 情况 下 的 目标 跟踪 进行 了 仿真 实验 。 踪 过 程 平稳 ,但 响应 缓慢 , 需要 长 的 跟踪 时 间 ， 如 图 8 所 示 。 
4.1 算法 描述 
为 了 解决 仓储 机 器 人 在 动态 环境 下 的 路 径 规划 ， 本 文 重 0 ns 
点 将 机 器 人 与 目标 点 、 障 得 物 的 相对 速度 和 相对 加 速度 引入 | xs CT 
势 场 函数 中 ， 从 而 得 到 了 改进 的 引力 和 斥 力 函 数 。 算 法 的 开 ee 马 
展 首先 需要 获取 机 器 人 自身 、 目 标 和 障碍 物 的 位 置 、 速 度 和 zol 台 oocg 
加 速度 信息 ; 其 次 , 计算 出 目标 对 机 器 人 的 引力 作用 ; 再 次 ， 机 
判读 机 器 人 是 否 处 于 障碍 物 斥 力作 用 范围 ， 如 果 在 则 计算 出 1 如 woo, 
相应 的 斥 力 值 ， 否 则 ， 斥 力作 用 为 零 ， 最 后 ， 计 算 机 器 人 受 oo 
到 的 合力 ， 以 计算 机 器 人 在 下 一 步 的 加 速度 和 速度 ， 最 终 实 加 | © 
现 跟 踊 目 标 运动 。 具 体 流 程 如 图 6 所 示 。 sl 
人 4 6 8 10 入 14 16 18 20 


X 轴 
图 7 =2,k,=3,k=3 时 路 径 规 划 
Fig.7 Path planning at k,=2,k,=3,ka=3 


30 


可 机 器 人 轨迹 


加 目标 轨迹 
25 上 
©, 
20 上 3 
We Ce) 
及 
和 15 


X 轴 
图 8 =4k=2,k =1 时 路 径 规划 
Fig.8 Path planning at Kk, =4,k,=2,k,=1 
可 见 在 追踪 目标 的 时 响应 速度 、 稳 定性 和 精确 性 存在 一 
定 的 矛盾 ,在 参数 选择 时 , 需要 折 中 考虑 ， 当 =4 、h=5 和 
=2 时， 跟踪 效果 最 为 理想 ， 在 T=6s 时 ， 机 器 人 就 跟踪 上 
图 6 改进 入 工 势 场 法 路 径 规 划 流程 目标 , 并 和 目标 保持 相同 的 运动 状态 , 路 径 规划 如 图 9 所 示 。 


Fig.6 Flow diagram of improved artificial potential field algorithm 
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30 一 一 r 一 一 r 一 下 30 -一 - 
| 七 机 如 人 加 这 | 3 机 器 人 轨迹 上 县 标 移动 方向 
障碍 物 1 轨迹 | 
25 25 障碍 物 2 轨迹 | 
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Fig.9 Path planning at k,=2,k,=5,ka=1 Fig. 11 Path planning at T=2.4s 
4.3 ”有 障碍 物 情 况 下 的 目标 跟踪 
在 动态 目标 环境 的 基础 上 ， 随 机 设 定 3 个 动态 障碍 物 来 "er 目标 入 | 
验证 仓储 机 器 人 具备 在 动态 环境 中 避 障 和 目标 跟踪 的 能 力 ， 25 和 aatid ee a | 
Ee 障碍 物 3 轨 迹 3s 机 器 人 位 置 ,性 向 
体 仿真 参数 如 表 2 所 示 。 oe | 
表 2 有 障碍 物 情况 下 仿真 参数 ”| 
Table 2 Simulation parameters with obstacle conditions 录 15 
> 
仿真 参数 名 称 及 符号 值 /( 单 位 ) 
目标 初始 位 置 ps (5,27)/(m) | 
目标 初始 速度 vs (0.4,-0.4)/(m/s) 8 
目标 初始 加 速度 as (0.4,-0.2)/(m/s’) | 
机 器 人 初始 速度 v (0,0)/(m/s) 0 
机 器 人 初始 加 速度 ar (0,0)/(m/s’) wa 
障碍 物 1 初始 位 置 p” (0.5,15)/(m) 图 12 T=3.3s 时 仿真 结果 
障碍 物 1 初始 速度 v2; (2.5,-2)/(/s) Fig. 12 Path planning at T=3.3s 
障碍 物 1 初始 加 速度 a” -0.1,0.15)/(m/s’) i | 
障碍 物 2 初始 位 置 p” (13,21)/(m) a SS 
物 1 轨 六 
障碍 物 2 初始 速度 v” C3,-D/(m/s) 和 上 || 加 诡 nz T-3.9s 机 器 人 位 置 
障 得 物 2 初始 加 速度 a” (0.6,0.05)/(m/s’) 6 me | 3 可口 
障碍 物 3 初始 位 置 p” (8.5,14)/(m) . gr > 
障 得 物 3 初始 速度 v9 (1,2)/Gm/s) OO 
障碍 物 3 初始 加 速度 a” (-0.2,-0.2)/(m/s?) 0 9 0 oe 机 器 人 位 置 
机 器 人 质量 10/(Kg) ma 00000 
规划 时 间 A 0.3/(s) 。 
最 大 移动 速度 wu 5S/(m/s) 
引力 正比 例 系 数 (kp,kv,ka) (2,5,1) 0 上 - - 
eR i a 0 2 4 6 8 10 3 14 
斥 力 正比 例 系数 (1mp,mvma) (4,4,3) Xx 四 
仓储 机 器 人 在 引力 和 斥 力 共同 作用 ， 避 开 障 碍 物 跟踪 图 13 T=3.9s 仿真 结果 
标 ， 有 具体 过 程 如 图 10~16 所 示 。 Fig. 13 Path planning at T=3.9s 
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Fig. 10 The path planning at T=1.8s 
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图 14 T=6s 仿真 结果 
Fig. 14 Path planning at T=6s 
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Fig. 15 Path planning at T=6.6s 
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图 16 T=13s 时 仿真 结果 

Fig. 16 Path planning at T=13s 
JT 见 ， 机 器 人 在 t=1.8s 时 进入 障碍 物 1 的 作用 范围 ， 在 
标 ， 机 器 人 在 t=3.3s 时 
障碍 物 2 的 作用 范围 ， 在 t=3.9s 时 避 开 了 障碍 物 2; 在 
时 机 器 人 遇 到 障碍 物 3，t=6.6s 时 机 器 人 避 开 障碍 物 3， 


| 


保持 
的 人 
径 规 
4.4 


环境 


果 ， 
轨迹 


跟踪 目标 ; 在 t=13s 时 机 器 人 与 目标 处 于 相同 位 置 ， 且 
相同 的 速度 和 加 速度 继续 运动 。 仿 真 结果 验证 了 改进 后 
工 势 场 法 能 很 好 的 解决 仓储 机 器 人 在 全 动态 环境 下 的 路 
划 问 题 ， 且 跟踪 效率 高 ， 路 径 光滑 
与 其 他 算法 比较 

为 了 进一步 验证 本 算法 的 优越 性 ， 采 用 文献 [16] 的 仿真 
进行 仿真 实验 ， 结 果 如 图 17(b) 所 示 。 

图 17(a) 为 文献 [16] 的 仿真 结果 ，(b) 为 本 算法 的 仿真 结 
由 图 可 见 在 t=10 s 时 已 经 实现 与 目标 同步 运动 ， 且 运动 
圆滑 ， 所 以 ， 本 算法 在 动态 障碍 物 避 障 和 动态 目标 追踪 


上 更 
5 


了 机 
环境 
器 人 
目标 
经 仿 


优势 。 
结束 语 


本 文 首 先 分 析 了 仓储 机 器 人 自主 导航 的 工作 原理 ， 给 出 
器 人 和 障碍 物 的 数学 模型 ， 搭 建 了 基于 2 维 激光 雷达 的 
感知 平台 ， 提 出 了 一 种 改进 的 人 工 势 场 法 ， 使 得 仓储 机 
备 在 全 动态 环境 中 自主 导航 的 能 力 。 将 机 器 人 相对 于 
、 障 但 物 的 位 置 、 速 度 和 加 速度 引入 到 传统 势 场 函数 中 ， 
真 验证 ， 仓 储 机 器 人 不 仅 能 有 效 避 开动 态 障 碍 物 ， 还 能 


精确 
中 葛 


地 跟踪 目标 机 器 人 ， 为 下 一 步 应 用 到 现实 无 人 仓储 环境 
定 了 坚实 基础 。 
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(a) 文献 [16] 的 仿真 结果 
(a)Result at paper [16] 
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(b) Result in this paper 
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Fig. 17 Result compared with other method 
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